/*****************************************************************************************/
/*  < vectornav.cpp is a program provides a connection to VectorNav, 
   
  This program also publishes multiple topics like
	a. vn_ypr_topic : to publish yaw, pitch and roll values
	b. vn_acceleration_topic : to publsih the acceleration data
	c. vn_angular_rate_topic : to publsih the angular rate data
	d. vn_quaternion_topic: to publish the quaternaions
	e. kalman_filter_data: to publish the kalman filter values from gyroscope
	f. imu_data : The topic of type "sensor_msgs" to use robot_pose_ekf package

<If you intend to use only for position estimation via ROS EKF Position (robot_pose_ekf) Package
    please run the vn_imu_data_pub program.>
 
    Copyright (C) <iroboApp - 2013>  
    
    <Authors: Anis Koubaa <akoubaa@coins-lab.org> , Yasir Javed <yasir.javed@coins-lab.org> 

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
/*****************************************************************************************/


/******************************** NOTES ***********************************************/
/* This cpp file produces the vectornav_node used to publish all topics ***************/
/**************************************************************************************/

#include "vectornav.h" //define the interface of the class

using namespace std;
VectorNav::VectorNav(const ros::NodeHandle& nh)
  : _VNNodeHandle (nh)
  {
        uint64_t time=0;
	
	Vn100 vn100; // the Vn100 Handle required for connection and getting data from vectornav
	VnQuaternion quaternion;// for reading Quaternion fron VN100. They named quaternion components with x,y,z and w in the structured defined in vn_kinmatics.h
        VnYpr ypr; // for Yaw pith role
	VnVector3 acceleration; // for reading accelration information from VN100
	VnVector3 angularRate; // for reading the angular rate from VN100
	VnMatrix3x3 cosineMatrix; // for reading the cosine matrix from VN100 
        VnQuaternion attitude; // for reading attitude from VN100 used for kalman_filter_data
	VnVector3 gyroscopeBias;//for reading gyroscopeBias from VN100 used for kalman_filter_data
              
	const char* const COM_PORT = "//dev//ttyUSB0"; // the connection port of vectornav
        const int BAUD_RATE = 115200; // baudrate at which vectornav works
    
        
        // open VN100 serial port
        int err_val = vn100_connect(&vn100, COM_PORT, BAUD_RATE);
        // if the port does not open, use the following command is terminal
        // sudo chmod 777 /dev/ttyUSB0
        // assuming that VN100 is connected to /dev/ttyUSB0 
	if  (err_val !=0){
	printf("[Error: VectorNav Constructor] Unable to open COM Port for VN-100. \n Possible Solutions:\n");
	printf("    1- Change permission of Serial Port of VN 100. \n       Try command [sudo chmod 777 /dev/ttyUSB0] if /dev/ttyUSB0 is the COM port for VN-100\n");
	printf("     2- Check the connection of VN-100 to Serial Port. \n");
	printf("     3- Check that the port number is VN-100 to Serial Port. \n");	   
	}
	else 	
        printf("[VectorNav Connection Success] COM_PORT = %s \n", COM_PORT);
	printf("[VectorNav Node] All topics from VN100 in port %s will be published \n", COM_PORT);

       /******** Delcare VN-100 publishers ****************/
       
       //Tell the roscore master that we are going to be publishing a message of type rosVectorNav/vn100_msg on the topic vn_ypr_topic.
	_vectornavYPRPublisher = _VNNodeHandle.advertise<rosVectorNav::vn100_msg>("vn_ypr_topic", 1);
	//create a message to be published for yaw, pitch roll
	rosVectorNav::vn100_msg vn100_topic_object;

       //Tell the roscore master that we are going to be publishing a message of type geometry_msgs::Quaternion on the topic vn_vspace_topic.
	_vectornavQuaternionPublisher = _VNNodeHandle.advertise<geometry_msgs::Quaternion>("vn_quaternion_topic", 1);
	//create a message to be published for quaternion
	geometry_msgs::Quaternion vn100_topic_quaternion_object;

	//Tell the roscore master that we are going to be publishing a message of type geometry_msgs::VnVector3_msg on the topic vn_acceleration_topic.
	_vectornavAccelerationPublisher = _VNNodeHandle.advertise<geometry_msgs::Vector3>("vn_acceleration_topic", 1);
	//create a message to be published for acceleration
	geometry_msgs::Vector3 vn100_accelration_topic_object;

	//Tell the roscore master that we are going to be publishing a message of type geometry_msgs::VnVector3_msg on the topic vn_acceleration_topic.
	_vectornavAngularRatePublisher = _VNNodeHandle.advertise<geometry_msgs::Vector3>("vn_angular_rate_topic", 1);
	//create a message to be published for angular rate
	geometry_msgs::Vector3 vn100_angular_rate_topic_object;

	_vectornavIMUPublisher = _VNNodeHandle.advertise<sensor_msgs::Imu>("imu_data", 10);
	//create a message to be published for angular rate
	sensor_msgs::Imu vn100_data_Imu_topic_object;

	_vectornavKalmanFilterPublisher = _VNNodeHandle.advertise<rosVectorNav::KalmanFilteredPosition_msg>("kalman_filter_data", 10);
	//create a message to be published for angular rate
	rosVectorNav::KalmanFilteredPosition_msg vn100_kalman_Position_topic_object;
	
	/******** Start and loop publishing ****************/
	
	while (ros::ok()){
	
	vn100_getKalmanFilterStateVector(&vn100, &attitude,&gyroscopeBias);
	vn100_kalman_Position_topic_object.x = attitude.x;
	vn100_kalman_Position_topic_object.y = attitude.y;
	vn100_kalman_Position_topic_object.z = attitude.z;
	vn100_kalman_Position_topic_object.w = attitude.w;
	vn100_kalman_Position_topic_object.bias1 = gyroscopeBias.c0;
	vn100_kalman_Position_topic_object.bias2 = gyroscopeBias.c1;
	vn100_kalman_Position_topic_object.bias3 = gyroscopeBias.c2;
	_vectornavKalmanFilterPublisher.publish(vn100_kalman_Position_topic_object);
	  //vn100_getCalibratedImuMeasurements(&vn100, VnVector3* magnetic, &accelerationVN, VnVector3* angularRate, double* temperature);
	// getting the directional consine matrix
	vn100_getDirectionCosineMatrix(&vn100, &cosineMatrix);
	// Publsishing the Quaternion VectorSpace
	vn100_getQuaternion(&vn100, &quaternion);
	vn100_topic_quaternion_object.x = quaternion.x; //q0 defined as x in vn_kinmatics.h of vectornav library
	vn100_topic_quaternion_object.y = quaternion.y; //q1 defined as y in vn_kinmatics.h of vectornav library
	vn100_topic_quaternion_object.z = quaternion.z; //q2 defined as y in vn_kinmatics.h of vectornav library
	vn100_topic_quaternion_object.w = quaternion.w; //q3 defined as y in vn_kinmatics.h of vectornav library
	// publishing the topic Quaternion
      _vectornavQuaternionPublisher.publish(vn100_topic_quaternion_object);

	  vn100_getYawPitchRoll(&vn100, &ypr);      
	  vn100_topic_object.yaw = ypr.yaw; //getting the yaw value
	  vn100_topic_object.pitch = ypr.pitch; //getting the pitch value
	  vn100_topic_object.roll = ypr.roll; //getting the roll value
  	_vectornavYPRPublisher.publish(vn100_topic_object);
  
	// publishing the topic YPR accelration that  shows Acceleration Measurements register.
	   vn100_getAcceleration(&vn100, &acceleration);
	   vn100_accelration_topic_object.x=acceleration.c0;
	   vn100_accelration_topic_object.y=acceleration.c1;
	   vn100_accelration_topic_object.z=acceleration.c2;
	   //publising the acceleration topic
	   _vectornavAccelerationPublisher.publish(vn100_accelration_topic_object);

	   	   // publishing the angular rate
	   vn100_getAngularRate(&vn100, &angularRate);
	   vn100_angular_rate_topic_object.x=angularRate.c0;
	   vn100_angular_rate_topic_object.y=angularRate.c1;
	   vn100_angular_rate_topic_object.z=angularRate.c2;
	  
	   //publising the acceleration topic
	   _vectornavAngularRatePublisher.publish(vn100_angular_rate_topic_object);

	  /************************************************************************************************/
	  /************* This part is for updating IMU data needed for robot_pose_ekf *********************/
	  /************************************************************************************************/
	   // for publishing the IMU data
	  // derived from http://www.ros.org/doc/api/microstrain_3dmgx2_imu/html/imu__node_8cc_source.html
	   
	  
	    //get acceleration values from vn-100
	    vn100_data_Imu_topic_object.linear_acceleration.x = acceleration.c0;
	    vn100_data_Imu_topic_object.linear_acceleration.y = acceleration.c1;
	    vn100_data_Imu_topic_object.linear_acceleration.z = acceleration.c2;
	  
	    //get angular rates values from vn-100
	    vn100_data_Imu_topic_object.angular_velocity.x = angularRate.c0;
	    vn100_data_Imu_topic_object.angular_velocity.y = angularRate.c1;
	    vn100_data_Imu_topic_object.angular_velocity.z = angularRate.c2;
	        
	    //get quaternion values from vn-100
	    vn100_data_Imu_topic_object.orientation.x = quaternion.x;
	    vn100_data_Imu_topic_object.orientation.y = quaternion.y;
	    vn100_data_Imu_topic_object.orientation.z = quaternion.z;
	    vn100_data_Imu_topic_object.orientation.w = quaternion.w;
	    
	    // setting orientation covariance. values are taken from example of microstrain_3dmgx2_imu
	    vn100_data_Imu_topic_object.orientation_covariance[0] = -1;
	    vn100_data_Imu_topic_object.orientation_covariance[1] = 0;
	    vn100_data_Imu_topic_object.orientation_covariance[2] = 0;
	    vn100_data_Imu_topic_object.orientation_covariance[3] = 0;
	    vn100_data_Imu_topic_object.orientation_covariance[4] = 1;
	    vn100_data_Imu_topic_object.orientation_covariance[5] = 0;
	    vn100_data_Imu_topic_object.orientation_covariance[6] = 0;
	    vn100_data_Imu_topic_object.orientation_covariance[7] = 0;
	    vn100_data_Imu_topic_object.orientation_covariance[8] = -1;
	    
	    //get current time
	    vn100_data_Imu_topic_object.header.stamp = ros::Time::now().fromNSec(time);
	    //publish the imu__node topic for robot_pose_ekf node 
	    _vectornavIMUPublisher.publish(vn100_data_Imu_topic_object);
	   
	}
	//if leaves the endless loop, disconnect from vn-100
	vn100_disconnect(&vn100);

}

/* Destructor */
VectorNav::~VectorNav()
{
  ROS_INFO("[VectorNav::~VectorNav()] Destructor Called! ");
}


// Main Function
int main (int argc, char **argv)
{
  ros::init(argc, argv, "VNbase_footprint");
  ROS_INFO("Welcome to VectorNav ROS Driver");
  VectorNav _self(ros::NodeHandle("")); 
  
  return 0;
}
